#include "device.h"
#include "modbus.h"
#include "math.h"
#include "speed_cntr.h"
#include "astar.h"
#include <intrins.h>

#ifndef IGNORE_Z
#define MAX_SLAVE_COUNT         1
#else
#define MAX_SLAVE_COUNT         0
#endif
#define SLAVE_ADDRESS(n)        (1 + (n))

bit pending_action = 0;         // 新动作
bit ref_changed = 0;            // 参考坐标系改变

// Z电机运行参数
bit z_pick_cup_state_changed;   // 抓杯传感器状态改变
bit holding_cup_fallen;         // 从有测试杯到无测试杯触发
bit holding_cup_risen;          // 从无测试杯到有测试杯触发
bit check_holding;              // 检测测试杯有无触发沿
bit pick_cup_fallen;            // 抓杯传感器下降沿
bit pick_cup_risen;             // 抓杯传感器上升沿，从不遮挡到遮挡
long z_buffer_step;             // 检测到抓杯到位传感器后继续前进位置

#if (PCB_VERSION == PCB_A2)
bit filter_level                = 0;        // m传感器滤波电平
uchar level                     = 0;        // m传感器电平
uchar m_status                  = 0;        // m电机状态
bit open_triggered              = 0;        // m打开抓手动作已经触发过
long m_pos                      = 0;        // m电机当前坐标
ushort m_duration               = 0;        // m运动经过的时长
ulong m_distance                = 0;        // m经过的位移
#endif
////////////////////////////////////////////////////////////////////////////////

void Update_IO()reentrant
{

#if (PCB_VERSION == PCB_A2)
    x_at_home = M_AT_HOME;
#endif
    y_at_home = Y_AT_HOME;
    z_at_home = Z_AT_HOME;

    if (pick_cup_sensor != IO_PICK_CUP)
    {
        pick_cup_sensor = !pick_cup_sensor;
        if (pick_cup_sensor)
            pick_cup_risen = 1;
        else
            pick_cup_fallen = 1;
    }

    if (holding_cup != IO_HOLDING_CUP)
    {
        holding_cup = !holding_cup;
        if (holding_cup)
            holding_cup_risen = 1;
        else
            holding_cup_fallen = 1;
    }
}

#if (PCB_VERSION == PCB_A2)
////////////////////////////////////////////////////////////////////////////////
// X电机指令(X 电机用于驱动抓手打开/关闭)

// X电机脉冲输出
void M_Step(uchar level)
{
    UNUSED(level);
    srd_m.output = 0;
}

// X电机更新定时器,频率输出
void M_Set_Timer(ushort tick)
{
    uchar current = SFRPAGE;
    SFRPAGE   = CONFIG_PAGE;
    P3SKIP    = 0x01;
    SFRPAGE  = ACTIVE_PAGE;
    PCA0CPH2 = tick;
    SFRPAGE  = current;
}

// X电机停止定时器
void M_Stop_Timer()
{
    uchar current = SFRPAGE;
    SFRPAGE  = ACTIVE_PAGE;
    PCA0CPM2  = 0x00;
    SFRPAGE  = current;
}

// 启动X电机
void Start_M_Motor()
{
    m_distance = 0;

    IO_M_DIR(srd_m.dir);
    IO_M_STEP = 0;
    IO_M_EN(1);
    IO_M_LOCK(0);

    M_Set_Timer(FREQ_TABLE(srd_m.speed_index));
    PCA0L    = 0x00;
    PCA0H    = 0x00;
    PCA0CPM2 = 0x46;

    _push_(SFRPAGE);
    SFRPAGE   = ACTIVE2_PAGE;
    TMR5L     = TMR4CAPL;
    TMR5H     = TMR4CAPH;
    TMR5CN   |= 0x04;
    _pop_(SFRPAGE);
}

// 停止X电机
void Stop_M_Motor()
{
    _push_(SFRPAGE);
    SFRPAGE   = ACTIVE2_PAGE;
    TMR5CN   &= ~0x04;
    _pop_(SFRPAGE);

    IO_M_STEP = 0;
    IO_M_LOCK(1);
    srd_m.output = 0;
    if (srd_m.dir == CCW)
        m_distance = -srd_m.step_count;
    else
        m_distance = srd_m.step_count;
    m_duration = TICK_TO_US(SystemTick() - srd_m.start_time)/1000;

    srd_m.need_cleanup = 0;
    srd_m.running = 0;
    open_triggered = 0;
}

// 执行复位指令时, 每一步调用一次
void M_Reset_Update()
{
#if M_HOME_FILTER_ENABLE
    level = (level << 1) | M_AT_HOME;
    if((level == 0xff) || level == 0)
        filter_level = (level != 0);
#else
    filter_level = M_AT_HOME;
#endif

    // 判定原点状态改变
    if (filter_level != srd_m.home_state)
    {
        srd_m.home_state = !srd_m.home_state;
        srd_m.hit_home = 1;
        // 如果电机从非零点位置复位，当前 m_pos 在零点矫正范围内, 才矫正零点
        if(srd_m.home_state)
        {
            if(m_pos < M_HOME_MIN_ERR_STEP)
            {
                srd_m.step_left = M_RESET_BUFFER_STEP;
                m_pos = 0;
            }
            else
            {
                // 可能发生了信号干扰,也可能电机发生了丢步
                srd_m.step_left = M_PICKER_OPENED_POS;
            }
        }
    }
    else
        m_pos += srd_m.step_offset;
}

// 复位指令停止时调用
void M_Reset_Cleanup()
{
    m_status = srd_m.hit_home ? STATUS_IDLE : M_STATUS_M_HOME_SENSOR_ERROR;

    Stop_M_Motor();
}

// 执行移动指令时, 每一步调用一次
void M_Move_To_Update()
{
#if M_HOME_FILTER_ENABLE
    level = (level << 1) | M_AT_HOME;
    if((level == 0xff) || level == 0)
        filter_level = (level != 0);
#else
    filter_level = M_AT_HOME;
#endif

    // 判定原点状态改变
    if (filter_level != srd_m.home_state)
    {
        srd_m.home_state = !srd_m.home_state;
        srd_m.hit_home = 1;
        m_pos = 0;
        srd_m.step_left = labs(srd_m.target);
    }
    else
        m_pos += srd_m.step_offset;
}

// 移动指令停止时调用
void M_Move_To_Cleanup()
{
    m_status = srd_m.hit_home ? STATUS_IDLE : M_STATUS_M_HOME_SENSOR_ERROR;
    m_status = (M_AT_HOME && m_pos >= M_PICKER_OPENED_POS) ? M_STATUS_M_HOME_SENSOR_ERROR : STATUS_IDLE;

    Stop_M_Motor();
}

void M_Init_Reset()
{
    srd_m.init = Start_M_Motor;
    srd_m.step = M_Step;
    srd_m.start_timer = M_Set_Timer;
    srd_m.stop_timer = M_Stop_Timer;
    srd_m.update = M_Reset_Update;
    srd_m.cleanup = M_Reset_Cleanup;
    srd_m.home_state = M_AT_HOME;
    filter_level = srd_m.home_state;
    level = srd_m.home_state ? 0xff : 0;
    srd_m.interp = 0;
}

void M_Init_Move(bit ignore_step_left)
{
    srd_m.init = Start_M_Motor;
    srd_m.step = M_Step;
    srd_m.start_timer = M_Set_Timer;
    srd_m.stop_timer = M_Stop_Timer;
    srd_m.update = M_Move_To_Update;
    srd_m.cleanup = M_Move_To_Cleanup;
    srd_m.home_state = M_AT_HOME;
    filter_level = srd_m.home_state;
    level = srd_m.home_state ? 0xff : 0;
    srd_m.interp = 0;
    srd_m.ignore_step_left = ignore_step_left;
    open_triggered = 0;
}

void Do_Action_Open_Picker_No_Wait( )
{
    long step = 0;
    step = (M_PICKER_OPENED_POS - m_pos) > M_MAX_DISTANCE_STEP ? M_MAX_DISTANCE_STEP : (M_PICKER_OPENED_POS - m_pos);

    M_Init_Move(0);
    srd_m.target = M_PICKER_OPENED_POS;
    speed_cntr_Move(&srd_m, step, M_MAX_SPEED);
    open_triggered = 1;
}

void Do_Action_Open_Picker( )
{
    long step = 0;
    if(open_triggered)
    {
        while(srd_m.running)
            Process_Main_Loop();
        return;
    }
    step = (M_PICKER_OPENED_POS - m_pos) > M_MAX_DISTANCE_STEP ? M_MAX_DISTANCE_STEP : (M_PICKER_OPENED_POS - m_pos);

    M_Init_Move(0);
    srd_m.target = M_PICKER_OPENED_POS;
    speed_cntr_Move(&srd_m, step, M_MAX_SPEED);
    while(srd_m.running)
        Process_Main_Loop();
}

void Do_Action_Close_Picker()
{
    // 在复位位置和M复位传感器未失效,不动作
    if(M_AT_HOME && m_status != M_STATUS_M_HOME_SENSOR_ERROR)
        return;

    M_Init_Reset();
    srd_m.target = -M_RESET_BUFFER_STEP;

    speed_cntr_Move(&srd_m, -M_MAX_DISTANCE_STEP, M_MAX_SPEED);

    // 等待M完成
    while(srd_m.running)
        Process_Main_Loop();
}

#endif

////////////////////////////////////////////////////////////////////////////////
// Z电机指令

// Z电机脉冲输出
void Z_Step(uchar level)
{
    UNUSED(level);
    srd_z.output = 0;
}

// Z电机更新定时器
void Z_Set_Timer(ushort tick)
{
    uchar current = SFRPAGE;
    SFRPAGE   = CONFIG_PAGE;
    P3SKIP    = 0;
    SFRPAGE  = ACTIVE_PAGE;
    PCA0CPH1 = tick;
    SFRPAGE  = current;
}

// Z电机停止定时器
void Z_Stop_Timer()
{
    uchar current = SFRPAGE;
    SFRPAGE  = ACTIVE_PAGE;
    PCA0CPM1  = 0x00;
    SFRPAGE  = current;
}

// 启动z电机
void Start_Z_Motor()
{
    z_distance = 0;

    IO_Z_DIR = Z_DIR_OUTPUT(srd_z.dir);
    IO_Z_STEP = 0;
    IO_Z_EN(1);
#if (PCB_VERSION == PCB_A2)
    IO_Z_LOCK(0);
#endif

    pick_cup_risen = 0;
    pick_cup_fallen = 0;
    holding_cup_risen = 0;
    holding_cup_fallen = 0;


    Z_Set_Timer(FREQ_TABLE(srd_z.speed_index));
    PCA0L    = 0x00;
    PCA0H    = 0x00;
    PCA0CPM1 = 0x46;

    _push_(SFRPAGE);
    SFRPAGE   = ACTIVE2_PAGE;
    TMR5L     = TMR4CAPL;
    TMR5H     = TMR4CAPH;
    TMR5CN   |= 0x04;
    _pop_(SFRPAGE);
}

// 停止Y电机
void Stop_Z_Motor()
{
    _push_(SFRPAGE);
    SFRPAGE   = ACTIVE2_PAGE;
    TMR5CN   &= ~0x04;
    _pop_(SFRPAGE);

#if (PCB_VERSION == PCB_A2)
    IO_Z_LOCK(1);
#endif
    IO_Z_STEP = 0;

    srd_z.output = 0;
    if (srd_z.dir == CCW)
        z_distance = -srd_z.step_count;
    else
        z_distance = srd_z.step_count;
    z_duration = TICK_TO_US(SystemTick() - srd_z.start_time)/1000;

    srd_z.need_cleanup = 0;
    srd_z.running = 0;
}

// 执行复位指令时, 每一步调用一次
void Z_Reset_Update()
{
    // 判定原点状态改变
    if (Z_AT_HOME != srd_z.home_state)
    {
        srd_z.home_state = !srd_z.home_state;
        srd_z.hit_home = 1;
        z_pos = 0;
        srd_z.step_left = Z_RESET_BUFFER_STEP;
    }
    else
        z_pos += srd_z.step_offset;
}

// 复位指令结束时调用
void Z_Reset_Cleanup()
{
    if (!srd_z.hit_home)
        server_status = XY_STATUS_Z_HOME_SENSOR_ERROR;

    Stop_Z_Motor();
}

// 执行移动指令时, 每一步调用一次
void Z_Move_To_Update()
{
    // 判定原点状态改变
    if (Z_AT_HOME != srd_z.home_state)
    {
        srd_z.home_state = !srd_z.home_state;
        srd_z.hit_home = 1;
        z_pos = 0;
        if (!srd_z.ignore_step_left)
        {
            srd_z.step_correction = labs(srd_z.target) - srd_z.step_left;
            srd_z.step_left = labs(srd_z.target);
        }
    }
    else
        z_pos += srd_z.step_offset;
}

// 移动指令结束时调用
void Z_Move_To_Cleanup()
{
    Stop_Z_Motor();
}

// 探测测试杯位置时, 每一步调用一次
void Z_Pick_Cup_Update()
{
    Update_IO();
    Z_Move_To_Update();

#if USE_PICK_CUP_SENSOR
    // 判定机械手抓杯传感器状态， 抓手到位或测试杯从无到有信号
    if (!z_pick_cup_state_changed && ( pick_cup_risen || (holding_cup_risen && check_holding)))
    {
        z_pick_cup_state_changed = 1;
        z_pick_cup_pos  = z_pos;
        if (srd_z.target > z_pos + z_buffer_step)
            srd_z.target = z_pos + z_buffer_step;
        srd_z.step_left = srd_z.target - z_pos;
    }

    // 去除holding_cup_fallen沿抖动干扰
    /*if (z_pick_cup_state_changed && holding_cup_risen && !holding_cup)
    {
        z_pick_cup_state_changed = 0;
        holding_cup_fallen = 0;
        srd_z.step_left = z_target - z_pos;
        if (srd_z.step_left < 0)
            srd_z.step_left = 0;
    }*/
#endif
}

// 抓杯回退时, 每一步调用一次
void Z_Rollback_Cup_Update()
{
    Z_Move_To_Update();

#if USE_PICK_CUP_SENSOR
    // 机械手抓杯传感器状态为0时, 停止电机
    if (!z_pick_cup_state_changed && pick_cup_fallen)
    {
        z_pick_cup_state_changed = 1;
        srd_z.step_left = PICK_CUP_ROLLBACK_STEP;
    }
#endif
}

////////////////////////////////////////////////////////////////////////////////
// Y电机指令

// Y电机脉冲输出
void Y_Step(uchar level)
{
    UNUSED(level);
    srd_y.output = 0;
}

// Y电机更新定时器
void Y_Set_Timer(ushort tick)
{
    uchar current = SFRPAGE;
    SFRPAGE  = ACTIVE_PAGE;
    PCA0CPH0 = tick;
    SFRPAGE  = current;
}

// Y电机停止定时器
void Y_Stop_Timer()
{
    uchar current = SFRPAGE;
    SFRPAGE  = ACTIVE_PAGE;
    PCA0CPM0  = 0x00;
    SFRPAGE  = current;
}

// 启动Y电机
void Start_Y_Motor()
{
    y_distance = 0;

    IO_Y_DIR = Y_DIR_OUTPUT(srd_y.dir);
    IO_Y_STEP = 0;
    IO_Y_EN(1);
#if (PCB_VERSION == PCB_A2)
    IO_Y_LOCK(0);
#endif

    Y_Set_Timer(FREQ_TABLE(srd_y.speed_index));
    PCA0L    = 0x00;
    PCA0H    = 0x00;
    PCA0CPM0 = 0x46;

    _push_(SFRPAGE);
    SFRPAGE   = ACTIVE2_PAGE;
    TMR4L     = TMR4CAPL;
    TMR4H     = TMR4CAPH;
    TMR4CN   |= 0x04;
    _pop_(SFRPAGE);
}

// 停止Y电机
void Stop_Y_Motor()
{
    _push_(SFRPAGE);
    SFRPAGE   = ACTIVE2_PAGE;
    TMR4CN   &= ~0x04;
    _pop_(SFRPAGE);

#if (PCB_VERSION == PCB_A2)
    IO_Y_LOCK(1);
#endif
    IO_Y_STEP = 0;

    srd_y.output = 0;
    if (srd_y.dir == CCW)
        y_distance = -srd_y.step_count;
    else
        y_distance = srd_y.step_count;
    y_duration = TICK_TO_US(SystemTick() - srd_y.start_time)/1000;

    srd_y.need_cleanup = 0;
    srd_y.running = 0;
}

// 执行复位指令时, 每一步调用一次
void Y_Reset_Update()
{
    // 判定原点状态改变
    if (Y_AT_HOME != srd_y.home_state)
    {
        srd_y.home_state = !srd_y.home_state;
        srd_y.hit_home = 1;
        y_pos = 0;
        srd_y.step_left = Y_RESET_BUFFER_STEP;
    }
    else
        y_pos += srd_y.step_offset;
}

// 复位指令结束时调用
void Y_Reset_Cleanup()
{
    if (!srd_y.hit_home)
        server_status = XY_STATUS_Y_HOME_SENSOR_ERROR;

    Stop_Y_Motor();
}

// 执行移动指令时, 每一步调用一次
void Y_Move_To_Update()
{
    // 判定原点状态改变
    if (Y_AT_HOME != srd_y.home_state)
    {
        srd_y.home_state = !srd_y.home_state;
        srd_y.hit_home = 1;
        y_pos = 0;
        if (!srd_y.ignore_step_left)
        {
            srd_y.step_correction = labs(srd_y.target) - srd_y.step_left;
            srd_y.step_left = labs(srd_y.target);
        }
    }
    else
        y_pos += srd_y.step_offset;
}

// 移动指令结束时调用
void Y_Move_To_Cleanup()
{
    Stop_Y_Motor();
}

////////////////////////////////////////////////////////////////////////////////

void Y_Init_Reset()
{
    srd_y.init = Start_Y_Motor;
    srd_y.step = Y_Step;
    srd_y.start_timer = Y_Set_Timer;
    srd_y.stop_timer = Y_Stop_Timer;
    srd_y.update = Y_Reset_Update;
    srd_y.cleanup = Y_Reset_Cleanup;
    srd_y.home_state = Y_AT_HOME;
    srd_y.interp = 0;
}

void Z_Init_Reset()
{
    srd_z.init = Start_Z_Motor;
    srd_z.step = Z_Step;
    srd_z.start_timer = Z_Set_Timer;
    srd_z.stop_timer = Z_Stop_Timer;
    srd_z.update = Z_Reset_Update;
    srd_z.cleanup = Z_Reset_Cleanup;
    srd_z.home_state = Z_AT_HOME;
    srd_z.interp = 0;
}

void Y_Init_Move(bit ignore_step_left)
{
    srd_y.init = Start_Y_Motor;
    srd_y.step = Y_Step;
    srd_y.start_timer = Y_Set_Timer;
    srd_y.stop_timer = Y_Stop_Timer;
    srd_y.update = Y_Move_To_Update;
    srd_y.cleanup = Y_Move_To_Cleanup;
    srd_y.home_state = Y_AT_HOME;
    srd_y.interp = 0;
    srd_y.ignore_step_left = ignore_step_left;
}

void Z_Init_Move(bit ignore_step_left)
{
    srd_z.init = Start_Z_Motor;
    srd_z.step = Z_Step;
    srd_z.start_timer = Z_Set_Timer;
    srd_z.stop_timer = Z_Stop_Timer;
    srd_z.update = Z_Move_To_Update;
    srd_z.cleanup = Z_Move_To_Cleanup;
    srd_z.home_state = Z_AT_HOME;
    srd_z.interp = 0;
    srd_z.ignore_step_left = ignore_step_left;
}

// 移动到指定坐标, 成功返回1
bit Do_Move_XY_To(long x, long y, ushort xspeed, ushort yspeed)
{
    Y_Init_Move(0);
    srd_y.target = y;
    UNUSED(x);
    UNUSED(xspeed);
    speed_cntr_Move(&srd_y, y - y_pos, yspeed);
    while(srd_y.running)
        Process_Main_Loop();

    return server_status == STATUS_IDLE;
}

bit Do_Z_Reset()
{
    // Z已经在零位, 动作完成
    if (Z_AT_HOME)
        return server_status == STATUS_IDLE;

    Z_Init_Reset();
    srd_z.target = srd_z.home_state ? Z_RESET_BUFFER_STEP : -Z_RESET_BUFFER_STEP;
    speed_cntr_Move(&srd_z, srd_z.home_state ? z_max_distance : -z_max_distance, z_max_speed);
    while(srd_z.running)
        Process_Main_Loop();

    if(!srd_z.hit_home)
        server_status = XY_STATUS_Z_HOME_SENSOR_ERROR;
    return server_status == STATUS_IDLE;
}

// 移动到指定坐标, 成功返回1
bit Do_Move_Z_To(long z, ushort zspeed)
{
    Z_Init_Move(0);
    srd_z.target = z;
    speed_cntr_Move(&srd_z, z - z_pos, zspeed);
    while(srd_z.running)
        Process_Main_Loop();

    return server_status == STATUS_IDLE;
}

////////////////////////////////////////////////////////////////////////////////
//debug
#ifdef PRODUCT_DEBUG
// 移动到指定坐标, 成功返回1
bit Do_Move_Y_To(long y,ushort yspeed)
{
    Y_Init_Move(0);
    srd_y.target = y;
    speed_cntr_Move(&srd_y, y - y_pos, yspeed);
    while(srd_y.running)
        Process_Main_Loop();
    return server_status == STATUS_IDLE;
}
#endif
////////////////////////////////////////////////////////////////////////////////

#if MOVE_METHOD == MOVE_METHOD_INDEPENDENT
typedef struct TimeData
{
    // 需要初始化的变量
    long pos;               // 起始位置, P1
    long target;            // 目标位置, P2
    long intermediate;      // 中间位置, P3
    ushort speed;           // 最高速度
    SpeedRampData *srd;

    // 计算过程
    ushort max_index;       // 最高速度索引
    long step;              // 全程
    long collision_step;    // 到碰撞的距离
    long collision_time;    // 到碰撞的时间
} TimeData;

#ifdef AVOID_OBSTACLES
// 运行指定步数所需的时间 //? 求取速度合
long time_of_step(TimeData *d, long step)
{
    long left;

    // 加速阶段
    if (step <= d->max_index)
        return SUM_TABLE(step);
    else
    {
        left = d->step - step;

        // 匀速阶段
        if (left > d->max_index)
            return SUM_TABLE(d->max_index)
                    + TIME_TABLE(d->max_index) * (step - d->max_index);

        // 减速阶段, 全程时间 - 加速left距离的时间
        else
            return ( ((long)SUM_TABLE(d->max_index) << 1)
                    + TIME_TABLE(d->max_index) * (d->step - (d->max_index << 1))
                    )
                    - SUM_TABLE(left);
    }
}

ushort lower_bound(ushort *table, ushort count, ushort val)
{
    ushort first = 0;
    ushort last = count;
    ushort mid;
    while(first+1 < last)
    {
        mid = (first + last) >> 1;
        if (val <= table[mid])
            last = mid;
        else
            first = mid;
    }

    return first;
}
// 给定的时间所运行的步数
long step_of_time(TimeData *d, long time)
{
    long const_time;

    // 加速阶段
    if (time <= SUM_TABLE(d->max_index))
        return lower_bound(sum_table,
                           d->max_index >> SPEED_INTERVAL_CODE,
                           time) << SPEED_INTERVAL_CODE;

    // 匀速时间
    const_time = (d->step - (d->max_index << 1)) * TIME_TABLE(d->max_index);

    // 匀速阶段
    if (time <= const_time + SUM_TABLE(d->max_index))
        return d->max_index + (time - SUM_TABLE(d->max_index))
                / TIME_TABLE(d->max_index);

    // 减速阶段
    time = const_time + (SUM_TABLE(d->max_index) << 1) - time;
    if (time > 0)
        return d->step - (lower_bound(sum_table,
                                      d->max_index >> SPEED_INTERVAL_CODE,
                                      time) << SPEED_INTERVAL_CODE);

    return d->step;
}

// 初始化TimeData
void time_data_init(TimeData *d)
{
    d->max_index = speed_cntr_max_index(d->speed);
    d->step = labs(d->target - d->pos);

    // 最大索引不超过一半步数
    d->collision_step = d->step >> 1;
    if (d->max_index > d->collision_step)
        d->max_index = d->collision_step;

    d->collision_step = labs(d->intermediate - d->pos);
    d->collision_time = time_of_step(d, d->collision_step);
}
#endif
#endif



/*
  XY移动时中间不停顿的算法

  +------------------+
  |     Tx           |
  | P1 +-+           |
  |  Ty| |           |
  +----+-+ P3        |
  |//////|           |
  |/WALL/|           |
  |//////|   + P2    |
  +------+-----------+

  从P1运行到P2, 会撞到墙, 因此需要X(或Y)先启动, Y(或X)后启动.

  算法:
  1. 如果(X1, Y2)在WALL内, 碰撞方向u=Y, v=X;
     如果(X2, Y1)在WALL内, 碰撞方向u=X, v=Y;
     若都不在WALL内, 直接运行到P2, 结束
  2. 计算P1到P3的时间Tu, Tv.
  3. 若Tv <= Tu, 直接运行到P2, 结束.
  4. 令Dt = Tv - Tu
  5. 计算Dt时间内v方向运行的步数Sv
  6. V方向启动
  7. Sv步后, U启动
 */

// 移动到指定坐标, 并躲避障碍, 成功返回1
bit Move_XY_To(long x, long y, ushort xspeed, ushort yspeed)
{
#ifdef AVOID_OBSTACLES

#if MOVE_METHOD == MOVE_METHOD_INDEPENDENT
    TimeData time_x, time_y;
    TimeData *u, *v;    // u: 碰撞轴; v: 不碰撞的轴
    long dt;
#endif


    if (Astar_Find_Path(x_pos, y_pos, x, y))
    {
#if MOVE_METHOD == MOVE_METHOD_INDEPENDENT
        if (astar_path.path_count == 1)     // ? 路径点数为1
        {
            time_x.pos = x_pos;
            time_x.target = x;
            time_x.intermediate = astar_path.path_x[0];
            time_x.speed = xspeed;
            time_x.srd = &srd_x;
            time_data_init(&time_x);

            time_y.pos = y_pos;
            time_y.target = y;
            time_y.intermediate = astar_path.path_y[0];
            time_y.speed = yspeed;
            time_y.srd = &srd_y;
            time_data_init(&time_y);

            // 碰撞方向
            if (Astar_Is_Wall(x_pos, y))
            {
                // Y方向碰撞
                u = &time_y;
                v = &time_x;
            }
            else
            {
                // X方向碰撞
                u = &time_x;
                v = &time_y;
            }

            // 驶出碰撞区与碰撞的时间差
            dt = v->collision_time - u->collision_time;

            // 碰撞前已经驶出碰撞区
            if (dt <= 0)
                return Do_Move_XY_To(x, y, xspeed, yspeed);

            // dt时间驶过的步数
            dt = step_of_time(v, dt);

            // 移动初始化
            Y_Init_Move(0);
            srd_y.target = y;
            X_Init_Move(0);
            srd_x.target = x;

            // 启动不碰撞的轴, 并等待距离超过dt
            speed_cntr_Move(v->srd, v->target - v->pos, v->speed);
            while(v->srd->running && v->srd->step_count < dt)
                Process_Main_Loop();

            // 如果出错, 返回
            if (server_status != STATUS_IDLE)
                return 0;

            // 启动碰撞轴
            speed_cntr_Move(u->srd, u->target - u->pos, u->speed);
            while(srd_x.running || srd_y.running)
                Process_Main_Loop();

            return server_status == STATUS_IDLE;
        }
#endif
        for(astar_path.path_index = 0;
            astar_path.path_index < astar_path.path_count;
            ++astar_path.path_index)
        {
            if (!Do_Move_XY_To(astar_path.path_x[astar_path.path_index],
                               astar_path.path_y[astar_path.path_index],
                               xspeed, yspeed))
                return 0;
        }
    }
#endif
    return Do_Move_XY_To(x, y, xspeed, yspeed);
}

////////////////////////////////////////////////////////////////////////////////

// 将Z移动到指定位置
bit Move_XYZ_To()
{
    // 所有Z回零: 如果XY不在目标位置或者需要探测液面
    if (y_target != y_pos)
    {
        if (!Do_Z_Reset())
            return 0;
    }

    // XY移动到拾取/释放测试杯位置
    if (!Move_XY_To(x_target,
                    y_target,
                    x_max_speed,
                    y_max_speed))
        return 0;

#ifndef IGNORE_Z
    if(!Do_Move_Z_To(z_target,z_max_speed))
        return 0;
#endif//IGNORE_Z

    return 1;
}

////////////////////////////////////////////////////////////////////////////////

void Do_Action_Stop()
{
    // 停止Y电机
    if (srd_y.running)
    {
        Y_Stop_Timer();
        Stop_Y_Motor();
    }
    // 停止Z电机
    if (srd_z.running)
    {
        Z_Stop_Timer();
        Stop_Z_Motor();
    }

    server_status = STATUS_IDLE;
}

void Do_Action_Reset()
{
    // z复位
    if (!Do_Z_Reset())
    {
        return;
    }

    // 复位Y轴
    Y_Init_Reset();
    srd_y.target = srd_y.home_state ? Y_RESET_BUFFER_STEP : -Y_RESET_BUFFER_STEP;
    speed_cntr_Move(&srd_y, srd_y.home_state ? y_max_distance : -y_max_distance,
                    y_max_speed < Y_MAX_RESET_SPEED ? y_max_speed : Y_MAX_RESET_SPEED);

    // 等待Y轴完成
    while(srd_y.running)
        Process_Main_Loop();

    // 判断Y复位状态
    if (server_status != STATUS_IDLE)
        return;

    server_status = STATUS_IDLE;
}

void Do_Action_Home_YX()
{
    // z复位
    if (!Do_Z_Reset())
    {
        return;
    }

    server_status = STATUS_IDLE;

    Update_IO();
    if(!y_at_home)
    {
        // 复位Y轴
        Y_Init_Reset();
        srd_y.target = srd_y.home_state ? Y_RESET_BUFFER_STEP : -Y_RESET_BUFFER_STEP;
        speed_cntr_Move(&srd_y, srd_y.home_state ? y_max_distance : -y_max_distance, y_max_speed);

        // 等待Y轴完成
        while(srd_y.running)
            Process_Main_Loop();

        // 判断Y复位状态
        if (server_status != STATUS_IDLE)
            return;
    }

    Update_IO();

    server_status = STATUS_IDLE;
}
// 抓杯时多走的相对位移, 步数
#define PICKUP_CUP_EXTRA_STEP   ((short)(PICKUP_CUP_EXTRA_OFFSET * SPR / Y_PERIMETER))

// 当USE_PICK_CUP_SENSOR = 0时, 该函数不会被调用, 之所以存在,
// 是因为当USE_PICK_CUP_SENSOR = 1时picker.uvproj中<OverlayString>设置中用到该函数
void Do_Action_Rollback_Cup(long target)
{
#if !USE_PICK_CUP_SENSOR
    // 没有任何用途, 只是为了消除编译警告
    target = Z_Rollback_Cup_Update;
#else
    z_pick_cup_state_changed = 0;

    Z_Init_Move(0);
    srd_z.update = Z_Rollback_Cup_Update;
    srd_z.target = target;
    srd_z.first_stage_step = 0;
    srd_z.second_stage_step = 0;

    speed_cntr_Move(&srd_z,
                    target - z_pos,
                    z_max_speed);

    while(srd_z.running)
        Process_Main_Loop();
#endif
}

//打开释放电磁铁 1:使能电磁铁，打开抓手； 0：关闭电磁铁，关闭抓手
void Do_Magent(bit action)
{
    IO_MAGENT = MAGENT_OUTPUT(action);
}

bit Do_Action_Z_Pick_Cup(long target)
{
    long z_pre_pos;
    z_pick_cup_pos = 0;
    z_pick_cup_state_changed = 0;
    z_buffer_step = Z_PROBE_BUFFER_STEP;
    check_holding = 0;
    z_pre_pos = z_pos;
    Z_Init_Move(0);
    srd_z.update = Z_Pick_Cup_Update;
    srd_z.target = target;

#if (PCB_VERSION == PCB_A2)
    Do_Action_Open_Picker();
#endif
    // 打开抓手
    Do_Magent(1);

    speed_cntr_Move(&srd_z,
                    target - z_pos,
                    z_max_speed);

    while(srd_z.running)
    {
        Process_Main_Loop();
    }

#if (PCB_VERSION == PCB_A2)
    Do_Action_Close_Picker();
#endif

    // 关闭抓手
    Do_Magent(0);

#if CHECK_PICK_CUP_SENSOR
    if (!IO_PICK_CUP)
     {
        server_status = XY_STATUS_PICKUP_CUP_SENSOR_ERROR;
        return 0;
    }
#endif

#if USE_PICK_CUP_SENSOR
    if (z_pick_cup_state_changed)
    {
    }
    else
    {
        server_status = XY_STATUS_PICKUP_CUP_SENSOR_ERROR;
        //return 0;
    }
#else
    // 为了消除编译警告, 下面函数没有任何动作
    Do_Action_Rollback_Cup(target);
#endif

    return 1;
}

// 机械手释放测试杯
bit Do_Action_Z_Release_Cup(long target)
{
    z_pick_cup_pos = 0;
    z_pick_cup_state_changed = 0;
    z_buffer_step = Z_RELEASE_BUFFER_STEP;
    check_holding = 0;

    Z_Init_Move(0);
    srd_z.update = Z_Pick_Cup_Update;
    srd_z.target = target;

    speed_cntr_Move(&srd_z,
                    target - z_pos,
                    z_max_speed);

    while(srd_z.running)
        Process_Main_Loop();

#if USE_PICK_CUP_SENSOR && CHECK_RELEASE_CUP_STATE
    if(flags & RELEASE_CUP_SENSOR_MASK)
    {
        // 忽略 PICK CUP SENSOR ERROR
    }
    else
    {
        // 抓杯命令检测到抓杯传感器后, 往回走一些步数
        if (z_pick_cup_state_changed)
        {
            /* target = z_pos - (Z_PROBE_BUFFER_STEP * 2 + PICK_CUP_ROLLBACK_STEP);
            if (target != z_pos && pick_cup_risen)
                Do_Action_Rollback_Cup(target);*/
        }
        else
        {
            server_status = XY_STATUS_RELEASE_CUP_ERROR;
            return 0;
        }
    }
#endif

#if (PCB_VERSION == PCB_A2)
    Do_Action_Open_Picker();
#endif

    // 打开抓手
    Do_Magent(1);

    return 1;
}

// 探测表面位置
void Do_Action_Probe_Level()
{
#if USE_PICK_CUP_SENSOR
    long target;
#endif

    // 所有Z回零
    if (!Do_Z_Reset())
        return;

    // XY移动到拾取/释放测试杯位置
    if (!Move_XY_To(x_target,
                    y_target,
                    x_max_speed,
                    y_max_speed))
        return;

    z_pick_cup_pos = 0;
    z_pick_cup_state_changed = 0;
    z_buffer_step = Z_PROBE_BUFFER_STEP;
    check_holding = 0;

    Z_Init_Move(0);
    srd_z.update = Z_Pick_Cup_Update;
    srd_z.target = z_target;

    speed_cntr_Move(&srd_z,
                    z_target - z_pos,
                    z_max_speed);

    while(srd_z.running)
        Process_Main_Loop();

#if USE_PICK_CUP_SENSOR
    // 抓杯命令检测到抓杯传感器后, 往回走一些步数
    if (z_pick_cup_state_changed)
    {
        target = z_pos - (Z_PROBE_BUFFER_STEP * 2 + PICK_CUP_ROLLBACK_STEP);

        if (target != z_pos && pick_cup_risen)
            Do_Action_Rollback_Cup(target);
    }
    else
    {
        server_status = XY_STATUS_PICKUP_CUP_ERROR;
    }
#endif
}

// 移动到xyz位置，然后拾取(pick=1)/释放(pick=0)测试杯
void Do_Action_Pick_Cup(bit pick)
{
    // 所有Z回零
    if (!Do_Z_Reset())
        return;

#if (PCB_VERSION == PCB_A2)
    open_triggered = 0;
    if(pick)
        Do_Action_Open_Picker_No_Wait();
#endif

    // XY移动到拾取/释放测试杯位置
    if (!Move_XY_To(x_target,
                    y_target,
                    x_max_speed,
                    y_max_speed))
        return;

#ifndef IGNORE_Z

    if (pick)
    {
        Do_Action_Z_Pick_Cup(z_target);
    }
    else
    {
        Do_Action_Z_Release_Cup(z_target);
    }

#endif// IGNORE_Z

#if RELEASE_CUP_DELAY
    // 放杯时, 延迟一会儿再平移
    if (!pick)
        Delay_Tick_Event(US_TO_TICK_UL(RELEASE_CUP_DELAY));
#endif// PICKUP_CUP_DELAY

#if PICKUP_CUP_DELAY
    // 抓杯时, 延迟一会儿再上移
    if (pick)
        Delay_Tick_Event(US_TO_TICK_UL(PICKUP_CUP_DELAY));
#endif// PICKUP_CUP_DELAY

#ifndef IGNORE_Z
    Z_Init_Reset();
    srd_z.target = srd_z.home_state ? Z_RESET_BUFFER_STEP : -Z_RESET_BUFFER_STEP;
    speed_cntr_Move(&srd_z, srd_z.home_state ? z_max_distance : -z_max_distance, z_max_speed);
    while(srd_z.running)
    {
        Process_Main_Loop();
    }

    if(!srd_z.hit_home)
    {
        if(server_status == STATUS_IDLE)
            server_status = XY_STATUS_Z_HOME_SENSOR_ERROR;
        return;
    }

    // 放杯完成后，关闭电磁铁
    if(!pick)
    {
    #if (PCB_VERSION == PCB_A2)
        Do_Action_Close_Picker();
    #endif
        Do_Magent(0);
        Delay_Tick_Event(US_TO_TICK_UL(PICKUP_CUP_DELAY));
    }

    // 检查HOLDING_CUP状态
    Update_IO();
    // 拾取/释放测试杯时, 如果机械手没有/有测试杯, 报错
    if (pick)
    {
        if(!holding_cup)
        {
            if(server_status == STATUS_IDLE)
                server_status = XY_STATUS_PICKUP_CUP_ERROR;
        }
    }
    else
    {

        if(holding_cup)
        {
            if(server_status == STATUS_IDLE)
                server_status = XY_STATUS_RELEASE_CUP_ERROR;
        }
    }

#endif// IGNORE_Z
}

#ifdef PRODUCT_DEBUG

// 从里侧孔抓杯到外侧，依次抓杯放杯
void Do_Pick_and_Release_Cup(unsigned char from,unsigned char n)
{
    char i;long y_stamp = y_target;
    if(from > 10 || from < 1)
        return;

    y_target = 0;
    x_target = 0;
    Do_Action_Pick_Cup(1);
    y_target = 0;// - 10;
    x_target = 480;
    Do_Action_Pick_Cup(0);
    while(1)
    {
        y_target = 0;// - 10;
        x_target = 480;
        for(i = from; i < (from + n);++i)
        {
            Do_Action_Pick_Cup(1);
            y_target = Y_STEPS_BY_ONE_HOLE * i - 10;
            Do_Action_Pick_Cup(0);
        }

        for(i = from + n - 2; i >= from - 1;--i)
        {
            Do_Action_Pick_Cup(1);
            y_target = Y_STEPS_BY_ONE_HOLE * i - 10;
            Do_Action_Pick_Cup(0);
        }
    }
}

#endif

#define MEASURE_MICROSTEP_2         0   // 2 细分定位
#define MEASURE_MICROSTEP_8         2   // 8 细分定位
#define MEASURE_MICROSTEP_16        3   // 16细分定位
#define MEASURE_MICROSTEP_32        4   // 32细分定位
#define MEASURE_MICROSTEP_64        5   // 16细分定位

#ifdef ENABLE_MEASURE_MICROSTEP
    #define MEASURE_MICROSTEP_CODE      MEASURE_MICROSTEP_2
#else
    #define MEASURE_MICROSTEP_CODE      MEASURE_MICROSTEP_2
#endif//ENABLE_MEASURE_MICROSTEP

#if MEASURE_MICROSTEP_CODE == MEASURE_MICROSTEP_64
    #define X_MOTOR_EXCITATION      X_MOTOR_EXCITATION_64
    #define Y_MOTOR_EXCITATION      Y_MOTOR_EXCITATION_64
#elif MEASURE_MICROSTEP_CODE == MEASURE_MICROSTEP_32
    #define X_MOTOR_EXCITATION      X_MOTOR_EXCITATION_32
    #define Y_MOTOR_EXCITATION      Y_MOTOR_EXCITATION_32
#elif MEASURE_MICROSTEP_CODE == MEASURE_MICROSTEP_16
    #define X_MOTOR_EXCITATION      X_MOTOR_EXCITATION_16
    #define Y_MOTOR_EXCITATION      Y_MOTOR_EXCITATION_16
#elif MEASURE_MICROSTEP_CODE == MEASURE_MICROSTEP_8
    #define X_MOTOR_EXCITATION      X_MOTOR_EXCITATION_8
    #define Y_MOTOR_EXCITATION      Y_MOTOR_EXCITATION_8
#else
    #undef  MEASURE_MICROSTEP_CODE
    #define MEASURE_MICROSTEP_CODE  MEASURE_MICROSTEP_2
    #define X_MOTOR_EXCITATION      X_MOTOR_EXCITATION_2
    #define Y_MOTOR_EXCITATION      Y_MOTOR_EXCITATION_2
#endif//MEASURE_MICROSTEP_CODE

// 测量次数, 至少2次
#define MEASURE_REPEAT              2

// X测量时的速度, 0.01 rad/sec
#define X_MEASURE_SPEED             ((ushort)(MEASURE_SPEED * 2 * 3.14159 * 100 / X_PERIMETER * (1 << MEASURE_MICROSTEP_CODE)))

// X测量时的速度, 0.01 rad/sec
#define Y_MEASURE_SPEED             ((ushort)(MEASURE_SPEED * 2 * 3.14159 * 100 / Y_PERIMETER * (1 << MEASURE_MICROSTEP_CODE)))

// 测试高度时, X距边的步数
#define X_MEASURE_OFFSET            ((ushort)(MEASURE_OFFSET * SPR / X_PERIMETER * (1 << MEASURE_MICROSTEP_CODE)))

// 测试高度时, Y距边的步数
#define Y_MEASURE_OFFSET            ((ushort)(MEASURE_OFFSET * SPR / Y_PERIMETER * (1 << MEASURE_MICROSTEP_CODE)))

#define SIDE_TOP                    0
#define SIDE_RIGHT                  1
#define SIDE_BOTTOM                 2
#define SIDE_LEFT                   3
#define SIDE_CENTER                 4
#define SIDE_NONE                   5

#define MEASURE_TOP                 (1 << SIDE_TOP)
#define MEASURE_RIGHT               (1 << SIDE_RIGHT)
#define MEASURE_BOTTOM              (1 << SIDE_BOTTOM)
#define MEASURE_LEFT                (1 << SIDE_LEFT)

#define MEASURE_HEIGHT_MASK         (0xF0)
#define MEASURE_HEIGHT_AT_TOP       (SIDE_TOP << 4)
#define MEASURE_HEIGHT_AT_RIGHT     (SIDE_RIGHT << 4)
#define MEASURE_HEIGHT_AT_BOTTOM    (SIDE_BOTTOM << 4)
#define MEASURE_HEIGHT_AT_LEFT      (SIDE_LEFT << 4)
#define MEASUER_HEIGHT_AT_CENTER    (SIDE_CENTER << 4)
#define MEASUER_HEIGHT_AT_NONE      (SIDE_NONE << 4)

#if defined(T0_RELOAD)
#define T0_MEASURE_BASE             (36)
#define T0_MEASURE_RELOAD_Y         (256 - T0_MEASURE_BASE)
#define T0_MEASURE_RELOAD_X         (256 - X_PERIMETER / Y_PERIMETER * T0_MEASURE_BASE)
#endif

void Do_Action_Disable_Motor()
{
    IO_Y_EN(0);
    IO_Z_EN(0);
#if (PCB_VERSION == PCB_A2)
    IO_M_EN(0);
#endif
}

void Process_Action()
{
    switch (action)
    {
    case XY_ACTION_STOP:
        Do_Action_Stop();
        ps_server_busy = 0;
        break;

    case XY_ACTION_RESET:
        ps_server_busy = 1;
        server_status = STATUS_IDLE;
        Do_Action_Reset();
        ps_server_busy = 0;
        break;

    case XY_ACTION_PICK_CUP:
        ps_server_busy = 1;
        server_status = STATUS_IDLE;
        Do_Action_Pick_Cup(1);
        ps_server_busy = 0;
        break;

    case XY_ACTION_RELEASE_CUP:
        ps_server_busy = 1;
        server_status = STATUS_IDLE;
        Do_Action_Pick_Cup(0);
        ps_server_busy = 0;
        break;

    case XY_ACTION_TRANSFER_CUP:
        ps_server_busy = 1;
        server_status = STATUS_IDLE;
        Do_Action_Pick_Cup(1);
        if (server_status == STATUS_IDLE)
        {
            x_target = x_target_2;
            y_target = y_target_2;
            z_target = z_target_2;
            Do_Action_Pick_Cup(0);
        }
        ps_server_busy = 0;
        break;

    case XY_ACTION_MOVE_XY:
        ps_server_busy = 1;
        server_status = STATUS_IDLE;
        Move_XYZ_To();
        ps_server_busy = 0;
        break;
    case XY_ACTION_PROBE_LEVEL:
        ps_server_busy = 1;
        server_status = STATUS_IDLE;
        Do_Action_Probe_Level();
        ps_server_busy = 0;
        break;
    case XY_ACTION_DISABLE_MOTOR:
        ps_server_busy = 1;
        server_status = STATUS_IDLE;
        Do_Action_Disable_Motor();
        ps_server_busy = 0;
        break;
    case XY_ACTION_MOVE_XYZ_0:
        ps_server_busy = 1;
        server_status = STATUS_IDLE;
        Move_XYZ_To();
        ps_server_busy = 0;
        break;
    case XY_ACTION_MOVE_XYZ_1:
        ps_server_busy = 1;
        server_status = STATUS_IDLE;
        Move_XYZ_To();
        ps_server_busy = 0;
        break;
    case XY_ACTION_MOVE_XYZ_2:
        ps_server_busy = 1;
        server_status = STATUS_IDLE;
        Move_XYZ_To();
        ps_server_busy = 0;
        break;
    case XY_ACTION_HOME_YX:
        ps_server_busy = 1;
        server_status = STATUS_IDLE;
        Do_Action_Home_YX();
        ps_server_busy = 0;
        break;
#if (PCB_VERSION == PCB_A2)
    case XY_ACTION_OPEN_PICKER:
        ps_server_busy = 1;
        server_status = STATUS_IDLE;
        Do_Action_Open_Picker();
        ps_server_busy = 0;
        break;
    case XY_ACTION_CLOSE_PICKER:
        ps_server_busy = 1;
        server_status = STATUS_IDLE;
        Do_Action_Close_Picker();
        ps_server_busy = 0;
        break;
#endif

#ifdef PRODUCT_DEBUG
    case XY_ACTION_RESET_Z:
        ps_server_busy = 1;
        server_status = STATUS_IDLE;
        Do_Z_Reset();
        ps_server_busy = 0;
        break;
    case XY_ACTION_MOVE_Z:
        ps_server_busy = 1;
        server_status = STATUS_IDLE;
        Do_Move_Z_To(z_target,z_max_speed);
        ps_server_busy = 0;
        break;
    case XY_ACTION_Z_PICK:
        ps_server_busy = 1;
        server_status = STATUS_IDLE;
        Do_Action_Z_Pick_Cup(z_target);
        ps_server_busy = 0;
        break;
    case XY_ACTION_MOVE_X:
        ps_server_busy = 1;
        server_status = STATUS_IDLE;
        ps_server_busy = 0;
        break;
    case XY_ACTION_MOVE_Y:
        ps_server_busy = 1;
        server_status = STATUS_IDLE;
        Do_Move_Y_To(y_target,y_max_speed);
        ps_server_busy = 0;
        break;
    case XY_ACTION_PICK_RELEASE_CUP:
        ps_server_busy = 1;
        server_status = STATUS_IDLE;
        Do_Pick_and_Release_Cup(1,9);
        ps_server_busy = 0;
        break;
#endif
    default:
        break;
    }
}
